import numpy as np
from scipy.integrate import odeint
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
from matplotlib import rcParams

rcParams['axes.unicode_minus'] = False
rcParams['font.family'] = 'simhei'

time = 10  # 秒
n = 100
t = np.linspace(0, time, n)#0-10 100个点  n*1的列向量
position = np.zeros((n, 6))   #n行6列的二维数组
dt = t[1] - t[0]

velocity, gamma, fai_v = 260., 3.14 / 10., 0.
x, y, z = 0, 0, 0  # 初始位置

# todo 三个控制量
control1 = 3.0
control2 = 2.
control3 = 3.14 / 12

position[0, 0] = x;
position[0, 1] = y;
position[0, 2] = z
position[0, 3] = velocity
position[0, 4] = gamma
position[0, 5] = fai_v


def dmove2(x_input, t, control):
    g = 9.81  # 重力加速度
    velocity, gamma, fai = x_input
    nx, nz, gunzhuan = control

    velocity_ = g * (nx - np.sin(gamma))  # # 米每秒
    gamma_ = (g / velocity) * (nz * np.cos(gunzhuan) - np.cos(gamma))  # 米每秒
    fai_ = g * nz * np.sin(gunzhuan) / (velocity * np.cos(gamma))

    return np.array([velocity_, gamma_, fai_])
for k in range(1, n):
    tspan = [t[k - 1], t[k]]
    po = odeint(dmove2, (velocity, gamma, fai_v), tspan, args=([control1, control2, control3],))
    velocity, gamma, fai_v = po[1, :]#取每一列的下标为1的行

    dx = velocity * np.cos(gamma) * np.sin(fai_v) * dt
    dy = velocity * np.cos(gamma) * np.cos(fai_v) * dt
    dz = velocity * np.sin(gamma) * dt

    x = x + dx;
    position[k, 0] = x
    y = y + dy;
    position[k, 1] = y
    z = z + dz;
    position[k, 2] = z

    position[k, 3] = velocity
    position[k, 4] = gamma
    position[k, 5] = fai_v

fig = plt.figure()
ax = Axes3D(fig)
plt.title('trajectory')
ax.plot(position[:, 0], position[:, 1], position[:, 2])
plt.xlabel("X")
plt.ylabel("Y")
plt.figure(3)
plt.title(r'velocity 速度')
plt.plot(position[:, 3])

plt.figure(4)
plt.title(r'theta  航迹倾角')
plt.plot(position[:, 4])

plt.figure(5)
plt.title(r'fai_v  航向角')
plt.plot(position[:, 5])
plt.show()